/// A quaternion.
/// This quaternion class is generic and may be non-unit, however most 
/// anticipated uses of quaternions are typically unit cases representing
/// a rotation 2*acos(w) about the axis (x,y,z).

#ifndef _QUATERNION_HPP_
#define _QUATERNION_HPP_

#include "Vector3D.hpp"
#include "Numerics.hpp"


namespace klasPhysics
{

class Quaternion
{
public:

	/// default constructor.
	/// does nothing for speed.

	Quaternion() {w = 0, x = 0, y = 0, z = 0;}

	/// construct quaternion from real component w and imaginary x,y,z.

	Quaternion(Real w, Real x, Real y, Real z)
	{
		this->w = w;
		this->x = x;
		this->y = y;
		this->z = z;
	}

	/// construct quaternion from angle-axis

	Quaternion(Real angle, const Vector3D &axis)
	{
		const Real a = angle * 0.5;
		const Real s = (Real) sin(a);
		const Real c = (Real) cos(a);
		w = c;
		x = axis.x * s;
		y = axis.y * s;
		z = axis.z * s;
	}

	/// construct quaternion from rotation matrix.

	/*Quaternion(const Matrix &matrix)
	{
		// Algorithm in Ken Shoemake's article in 1987 SIGGRAPH course notes
		// article "Quaternion Calculus and Fast Animation".

		const Real trace = matrix.m11 + matrix.m22 + matrix.m33;

		if (trace>0)
		{
			// |w| > 1/2, may as well choose w > 1/2

			Real root = sqrt(trace + 1.0);  // 2w
			w = 0.5 * root;
			root = 0.5 / root;  // 1/(4w)
			x = (matrix.m32-matrix.m23) * root;
			y = (matrix.m13-matrix.m31) * root;
			z = (matrix.m21-matrix.m12) * root;
		}
		else
		{
			// |w| <= 1/2

			static int next[3] = { 2, 3, 1 };
			
			int i = 1;
			if (matrix.m22>matrix.m11)  i = 2;
			if (matrix.m33>matrix(i,i)) i = 3;
			int j = next[i];
			int k = next[j];
			
			Real root = sqrt(matrix(i,i)-matrix(j,j)-matrix(k,k) + 1.0);
			Real *quaternion[3] = { &x, &y, &z };
			*quaternion[i] = 0.5 * root;
			root = 0.5 / root;
			w = (matrix(k,j)-matrix(j,k))*root;
			*quaternion[j] = (matrix(j,i)+matrix(i,j))*root;
			*quaternion[k] = (matrix(k,i)+matrix(i,k))*root;
		}
	}*/

	/// convert quaternion to matrix.

	/*Matrix matrix() const
	{
		// from david eberly's sources used with permission.
		
		Real fTx  = 2.0*x;
		Real fTy  = 2.0*y;
		Real fTz  = 2.0*z;
		Real fTwx = fTx*w;
		Real fTwy = fTy*w;
		Real fTwz = fTz*w;
		Real fTxx = fTx*x;
		Real fTxy = fTy*x;
		Real fTxz = fTz*x;
		Real fTyy = fTy*y;
		Real fTyz = fTz*y;
		Real fTzz = fTz*z;

		return Matrix(1.0-(fTyy+fTzz), fTxy-fTwz, fTxz+fTwy,
					  fTxy+fTwz, 1.0-(fTxx+fTzz), fTyz-fTwx,
					  fTxz-fTwy, fTyz+fTwx, 1.0-(fTxx+fTyy));
	}*/

	/// convert quaternion to angle-axis.

	void angleAxis(Real &angle, Vector3D &axis) const
	{
		const Real squareLength = x*x + y*y + z*z;
		
		if (squareLength>.000000001)
		{
			angle = 2.0 * (Real) acos(w);
			const Real inverseLength = 1.0 / (Real) pow(squareLength, 0.5);
			axis.x = x * inverseLength;
			axis.y = y * inverseLength;
			axis.z = z * inverseLength;
		}
		else
		{
			angle = 0.0;
			axis.x = 1.0;
			axis.y = 0.0;
			axis.z = 0.0;
		}
	}

	/// set quaternion to zero.

	void zero()
	{
		w = 0;
		x = 0;
		y = 0;
		z = 0;
	}

	/// set quaternion to identity.

	void identity()
	{
		w = 1;
		x = 0;
		y = 0;
		z = 0;
	}
	
	/// add another quaternion to this quaternion.

	void add(const Quaternion &q)
	{
		w += q.w;
		x += q.x;
		y += q.y;
		z += q.z;
	}

	/// subtract another quaternion from this quaternion.

	void subtract(const Quaternion &q)
	{
		w -= q.w;
		x -= q.x;
		y -= q.y;
		z -= q.z;
	}

	/// multiply this quaternion by a scalar.

	void multiply(Real s)
	{
		w *= s;
		x *= s;
		y *= s;
		z *= s;
	}

	/// divide this quaternion by a scalar.

	void divide(Real s)
	{
		kAssert(s!=0);
		const Real inv = 1.0 / s;
		w *= inv;
		x *= inv;
		y *= inv;
		z *= inv;
	}

	/// multiply this quaternion with another quaternion.

	void multiply(const Quaternion &q)
	{
		const Real rw = w*q.w - x*q.x - y*q.y - z*q.z;
		const Real rx = w*q.x + x*q.w + y*q.z - z*q.y;
		const Real ry = w*q.y - x*q.z + y*q.w + z*q.x;
		const Real rz = w*q.z + x*q.y - y*q.x + z*q.w;
		w = rw;
		x = rx;
		y = ry;
		z = rz;
	}

	/// multiply this quaternion with another quaternion and store result in parameter.

	void multiply(const Quaternion &q, Quaternion &result) const
	{
		result.w = w*q.w - x*q.x - y*q.y - z*q.z;
		result.x = w*q.x + x*q.w + y*q.z - z*q.y;
		result.y = w*q.y - x*q.z + y*q.w + z*q.x;
		result.z = w*q.z + x*q.y - y*q.x + z*q.w;
	}

	/// dot product of two quaternions.

	Quaternion dot(const Quaternion &q)
	{
		return Quaternion(w*q.w + x*q.x + y*q.y + z*q.z, 0, 0, 0);
	}

	/// dot product of two quaternions writing result to parameter.

	void dot(const Quaternion &q, Quaternion &result)
	{
		result = Quaternion(w*q.w + x*q.x + y*q.y + z*q.z, 0, 0, 0);
	}

	/// calculate conjugate of quaternion.

	Quaternion conjugate()
	{
		return Quaternion(w, -x, -y, -z);
	}

	/// calculate conjugate of quaternion and store result in parameter.

	void conjugate(Quaternion &result) const
	{
		result = Quaternion(w, -x, -y, -z);
	}

	/// calculate length of quaternion

	Real length() const
	{
		return sqrt(w*w + x*x + y*y + z*z);
	}

	/// calculate norm of quaternion.

	Real norm() const
	{
		return w*w + x*x + y*y + z*z;
	}

	/// normalize quaternion.

	void normalize()
	{
		const Real length = this->length();

		if (length == 0)
		{
			w = 1;
			x = 0;
			y = 0;
			z = 0;
		}
		else
		{
			Real inv = 1.0 / length;
			x = x * inv;
			y = y * inv;
			z = z * inv;
			w = w * inv;
		}
	}

	/// check if quaternion is normalized

	bool normalized() const
	{
		return IsEqual(norm(),1);
	}

	/// calculate inverse of quaternion

	Quaternion inverse() const
	{
		const Real n = norm();
		kAssert(n!=0);
		return Quaternion(w/n, -x/n, -y/n, -z/n); 
	}

	/// calculate inverse of quaternion and store result in parameter.

	void inverse(Quaternion &result) const
	{
		const Real n = norm();
		result = Quaternion(w/n, -x/n, -y/n, -z/n);
	}

	/// equals operator

	bool operator ==(const Quaternion &other) const
	{
		if ( IsEqual(w,other.w) && IsEqual(x,other.x) && IsEqual(y,other.y) && IsEqual(z,other.z)) 
			return true;
		else 
			return false;
	}

	/// not equals operator

	bool operator !=(const Quaternion &other) const
	{
		return !(*this==other);
	}
	
	/// element access

	Real& operator [](int i)
	{
		kAssert(i>=0);
		kAssert(i<=2);
		return *(&w+i);
	}

	/// element access (const)

	const Real& operator [](int i) const
	{
		kAssert(i>=0);
		kAssert(i<=2);
		return *(&w+i);
	}

	friend inline Quaternion operator-(const Quaternion &a);
	friend inline Quaternion operator+(const Quaternion &a, const Quaternion &b);
	friend inline Quaternion operator-(const Quaternion &a, const Quaternion &b);
	friend inline Quaternion operator*(const Quaternion &a, const Quaternion &b);
	friend inline Quaternion& operator+=(Quaternion &a, const Quaternion &b);
	friend inline Quaternion& operator-=(Quaternion &a, const Quaternion &b);
	friend inline Quaternion& operator*=(Quaternion &a, const Quaternion &b);

	friend inline bool operator ==(const Quaternion &q, Real scalar);
	friend inline bool operator !=(const Quaternion &q, Real scalar);
	friend inline bool operator ==(Real scalar, const Quaternion &q);
	friend inline bool operator !=(Real scalar, const Quaternion &q);

	friend inline Quaternion operator*(const Quaternion &a, Real s);
	friend inline Quaternion operator/(const Quaternion &a, Real s);
	friend inline Quaternion& operator*=(Quaternion &a, Real s);
	friend inline Quaternion& operator/=(Quaternion &a, Real s);
	friend inline Quaternion operator*(Real s, const Quaternion &a);
	friend inline Quaternion& operator*=(Real s, Quaternion &a);

	Real w;        ///< w component of quaternion
	Real x;        ///< x component of quaternion
	Real y;        ///< y component of quaternion
	Real z;        ///< z component of quaternion
};


inline Quaternion operator-(const Quaternion &a)
{
	return Quaternion(-a.w, -a.x, -a.y, -a.z);
}

inline Quaternion operator+(const Quaternion &a, const Quaternion &b)
{
	return Quaternion(a.w+b.w, a.x+b.x, a.y+b.y, a.z+b.z);
}

inline Quaternion operator-(const Quaternion &a, const Quaternion &b)
{
	return Quaternion(a.w-b.w, a.x-b.x, a.y-b.y, a.z-b.z);
}

inline Quaternion operator*(const Quaternion &a, const Quaternion &b)
{
	return Quaternion( a.w*b.w - a.x*b.x - a.y*b.y - a.z*b.z, 
					   a.w*b.x + a.x*b.w + a.y*b.z - a.z*b.y,
					   a.w*b.y - a.x*b.z + a.y*b.w + a.z*b.x,
					   a.w*b.z + a.x*b.y - a.y*b.x + a.z*b.w );
}

inline Quaternion& operator+=(Quaternion &a, const Quaternion &b)
{
	a.w += b.w;
	a.x += b.x;
	a.y += b.y;
	a.z += b.z;
	return a;
}

inline Quaternion& operator-=(Quaternion &a, const Quaternion &b)
{
	a.w -= b.w;
	a.x -= b.x;
	a.y -= b.y;
	a.z -= b.z;
	return a;
}

inline Quaternion& operator*=(Quaternion &a, const Quaternion &b)
{
	a.multiply(b);
	return a;
}


inline bool operator ==(const Quaternion &q, Real scalar)
{
	if ( IsEqual(q.w,scalar) && IsEqual(q.x,0.0) && IsEqual(q.y,0.0) && IsEqual(q.z,0.0)) 
		return true;
	else 
		return false;
}

inline bool operator !=(const Quaternion &q, Real scalar)
{
	return !(q==scalar);
}

inline bool operator ==(Real scalar, const Quaternion &q)
{
	if ( IsEqual(q.w,scalar) && IsEqual(q.x,0) && IsEqual(q.y,0) && IsEqual(q.z,0)) 
		return true;
	else 
		return false;
}

inline bool operator !=(Real scalar, const Quaternion &q)
{
	return !(q==scalar);
}

inline Quaternion operator*(const Quaternion &a, Real s)
{
	return Quaternion(a.w*s, a.x*s, a.y*s, a.z*s);
}

inline Quaternion operator/(const Quaternion &a, Real s)
{
	return Quaternion(a.w/s, a.x/s, a.y/s, a.z/s);
}

inline Quaternion& operator*=(Quaternion &a, Real s)
{
	a.multiply(s);
	return a;
}

inline Quaternion& operator/=(Quaternion &a, Real s)
{
	a.divide(s);
	return a;
}

inline Quaternion operator*(Real s, const Quaternion &a)
{
	return Quaternion(a.w*s, a.x*s, a.y*s, a.z*s);
}

inline Quaternion& operator*=(Real s, Quaternion &a)
{
	a.multiply(s);
	return a;
}

/*Quaternion slerp(const Quaternion &a, const Quaternion &b, Real t) 
{
	kAssert(t>=0);
	kAssert(t<=1);
			
	Real flip = 1;

	Real cosine = a.w*b.w + a.x*b.x + a.y*b.y + a.z*b.z;
	
	if (cosine<0) 
	{ 
		cosine = -cosine; 
		flip = -1; 
	} 
	
	if ((1-cosine)<epsilon) 
		return a * (1-t) + b * (t*flip); 
	
	Real theta = (Real)acos(cosine); 
	Real sine = (Real)sin(theta); 
	Real beta = (Real)sin((1-t)*theta) / sine; 
	Real alpha = (Real)sin(t*theta) / sine * flip; 
	
	return a * beta + b * alpha; 
} */

}


#endif // _QUATERNION_HPP_